/*
 * Compare.c
 *
 *  Created on: 2012. 3. 14.
 *      Author: root
 */
#include "../includes.h"




void init_str_kalman(Kalman_filter * filter)
{

	filter->A = 1;
	filter->Q = 0.01;
	filter->H = 1;
	filter->R = 1;

	filter->B = 0;
	filter->u = 0;
	filter->x = 0;
	filter->P = 0;

	filter->run_cnt = 0; //Q =0.5; R=0.5;
}

double push_v(Kalman_filter * f, double v)
{
		double K;
		f->z = v;
		//first run
		if(!f->run_cnt){
			f->x = (1/f->H) * f->z;
			f->P = (1/f->H)* f->R * (1/f->H);
		}
		else{
			f->x = (f->A * f->x) + (f->B * f->u);
			f->P = (f->A * f->P * f->A) + f->Q;
			K = f->P * f->H * 1/(f->H * f->P * f->H + f->R);
			f->x = f->x + K * (f->z - f->H * f->x);
			f->P = f->P - K * f->H * f->P;
		}
		f->run_cnt++;
		return f->x;
}

double 	get_k(Kalman_filter * f){ return f->x; };
void 	set_k(Kalman_filter * f, double q, double r)
{
	f->Q = q;
	f->R = r;
};

Kalman_filter filter_M, filter_S;

void Calc_cdt(int i,int j,int sat,double *c_dt)
{
	double v;
	double dx,dy,dz, range;

//	myObs[0].d[i].diff[sat].L[0] = myObs[0].d[i].obs[sat].L[0] - myObs[1].d[j].obs[sat].L[0];
//	myObs[0].d[i].diff[sat].P[0] = myObs[0].d[i].obs[sat].P[0] - myObs[1].d[j].obs[sat].P[0];
//	myObs[0].d[i].diff[sat].L[1] = myObs[0].d[i].obs[sat].L[1] - myObs[1].d[j].obs[sat].L[1];
//	myObs[0].d[i].diff[sat].P[1] = myObs[0].d[i].obs[sat].P[1] - myObs[1].d[j].obs[sat].P[1];

	//c*dT = P + c*dt - d(ion) - d(trop) - p
	///////////////////////
	///////////////////////
	dx = myObs[0].d[i].corr[sat].x - myObs[0].pos[0]; //my_pos[0][0];
	dy = myObs[0].d[i].corr[sat].y - myObs[0].pos[1]; //my_pos[0][1];
	dz = myObs[0].d[i].corr[sat].z - myObs[0].pos[2]; //my_pos[0][2];
	v = dx*dx + dy*dy + dz*dz ;

	range = sqrt( v );
	v =  myObs[0].d[i].obs[sat].L[0] ;
	v -= range;
	v += myObs[0].d[i].corr[sat].corr_clk;
	v -=( myObs[0].d[i].corr[sat].corr_ion + myObs[0].d[i].corr[sat].corr_trop );	//ionotype	sewon 12-07-04

	c_dt[0] = v;


	///////////////////////
	///////////////////////
	dx = myObs[1].d[j].corr[sat].x - myObs[1].pos[0]; //my_pos[1][0];
	dy = myObs[1].d[j].corr[sat].y - myObs[1].pos[1]; //my_pos[1][1];
	dz = myObs[1].d[j].corr[sat].z - myObs[1].pos[2]; //my_pos[1][2];
	v = dx*dx + dy*dy + dz*dz ;

	range = sqrt( v );
	v =  myObs[1].d[j].obs[sat].L[0] ;
	v -= range;
	v += myObs[1].d[j].corr[sat].corr_clk;
	v -=( myObs[1].d[j].corr[sat].corr_ion + myObs[1].d[j].corr[sat].corr_trop );
	c_dt[1] = v;

/*
	dbgCV(129) printf("\n (%.5lf, %.5lf, %.5lf) (%.5lf, %.5lf, %.5f) \n",
	myObs[0].pos[0],myObs[0].pos[1],myObs[0].pos[2],
	myObs[1].pos[0],myObs[1].pos[1],myObs[1].pos[2]);
*/

	///////////////////////
	///////////////////////
	v = myObs[0].timeInterval[i] - myObs[1].timeInterval[j];
	//v = myObs[0].timeInterval[i];
	c_dt[2] = v;//*1e-9;

}


double TI_Push(check_TI *Ti, double v)
{
	if( !Ti->iMatch )
	{
		Ti->prev = v;
		Ti->iMatch++;
		return v;
	}
	else
	{
		if( fabs(v- Ti->prev)> 10000 ) // in ns
		{
			Ti->iMissMatch++;
			if( Ti->iMatch > 20)
			{
				if( Ti->iMissMatch > 10 )
				{
					Ti->iMatch = 0;
					Ti->iMissMatch = 0;
					Ti->prev = v;
					return v;
				}
				else return Ti->prev;
			}
			else return Ti->prev;
		}
		else{
			Ti->prev = v;
			Ti->iMatch++;
			Ti->iMissMatch = 0;
			return v;
		}
	}
}


check_TI ckTI;



int x_value[2]={0};
int bCompareFirst=1;
double dbRlt_global=0;
int iCvValid = 0;
int bCvValid = 0;
void Compare()
{

//	int argc;
	int a,b,c;
	double range; // real range
	double v;
	double c_dt[2+1];
	double dx,dy,dz;
	double dbRlt[10];
	int i,j,sat;
	double cdt_cnt;
	double cdt_sum[2+1];
	double dbCutLine = 2000;
	int ci;
	_CV_Data *CV = &VME.CV_Data;
	gtime_t t_time[2];
	_st_obs *MPos= &myObs[MDATA];
	_st_obs *SPos= &myObs[SDATA];
	FILE* fp;

	//sewon 120622
	//===CV Master&Slave Compare===//
	if( (SYSTEM.EB.LOCKMODE==EB_LOCKED) && (CONFIG.NETSEND==0)){

		if((MPos->pos[0]==0)||(MPos->pos[1]==0)||(MPos->pos[2]==0))	{
			if( SYSTEM.IO.CTRL_PORT.MODE == UDEBUG )	printf("CV Master Position Invalid\n");
			return;
		}
		if((SPos->pos[0]==0)||(SPos->pos[1]==0)||(SPos->pos[2]==0))	{
			if( SYSTEM.IO.CTRL_PORT.MODE == UDEBUG )	printf("CV Slave Position Invalid\n");
			return;
		}
	}

	if(bCompareFirst)
	{
		bCompareFirst = 0;

		x_value[0] = x_value[1] = 0;
		init_str_kalman(&filter_M);		//UDP DATA
		init_str_kalman(&filter_S);    //OEMV3

		set_k(&filter_S, 0.1, 25.0);
		set_k(&filter_M, 0.1, 25.0);
		myObs[0].iT = 0;
		myObs[1].iT = 0;
	}
	x_value[0]=0;
	//for(int sat = 1; sat<=32; sat++){
   // for(int fnum = 0; fnum<=1; fnum++){
	if(iLsiDebugVariable ==3)
	printf("###M:Head/Tail:%2d/%2d =%2d , S:Head/Tail:%2d/%2d =%2d\n"
			, myObs[0].iH,myObs[0].iT
			,(myObs[0].iH-myObs[0].iT+M_ST_OBS_SIZE)%M_ST_OBS_SIZE
			,myObs[1].iH,myObs[1].iT
			,(myObs[1].iH-myObs[1].iT+M_ST_OBS_SIZE)%M_ST_OBS_SIZE
			);

	for(i = myObs[0].iT; i!=myObs[0].iH; i = (i+1)%M_ST_OBS_SIZE){

		//printf("myObs[1].iT = %d \n",myObs[1].iT);

		cdt_cnt = 0; cdt_sum[0] = 0; cdt_sum[1] = 0; cdt_sum[2] = 0;
		for(j = myObs[1].iT; j!=myObs[1].iH ; j=(j+1)%M_ST_OBS_SIZE){
			for(sat = 0; sat<32; sat++){
				//myObs[0].c_dt[i]=myObs[1].c_dt[j]=0;
				dbgCV(3) printf("%d: %d, %d : %d\n",myObs[0].d[i].b[sat],  myObs[0].d[i].b_corr[sat]
																					,myObs[1].d[j].b[sat], myObs[1].d[j].b_corr[sat]);

				if( myObs[0].d[i].b[sat] && myObs[0].d[i].b_corr[sat] &&  myObs[1].d[j].b[sat] && myObs[1].d[j].b_corr[sat]
					//&& myObs[0].d[i].obs[sat].P[0] && myObs[1].d[i].obs[sat].P[0]
				){
					dbgCV(3) printf("%d <><> %d\n",(int)myObs[0].d[i].time.time,(int)myObs[1].d[j].time.time );
					dbgCV(126)	printf("%lf,%lf\n",myObs[0].d[i].obs[sat].L[0],myObs[1].d[j].obs[sat].L[0]);
					if(		myObs[0].d[i].time.time == myObs[1].d[j].time.time
							&& myObs[0].d[i].obs[sat].L[0] && myObs[1].d[j].obs[sat].L[0]
					)
					{

						myObs[0].iT = (i+1)%M_ST_OBS_SIZE;
						myObs[1].iT = (j+1)%M_ST_OBS_SIZE;
;
					}
					else continue;

					Calc_cdt(i,j,sat,c_dt);

					if( fabs(c_dt[0])>dbCutLine  || fabs(c_dt[1])>dbCutLine
							|| (!myObs[0].d[i].obs[sat].L[0] || !myObs[1].d[j].obs[sat].L[0] )
					)
					{
//						s += AnsiString().sprintf(
//								",%2d"
//								", %2.1f, %2.1f"
//								,sat+1
//								,0,0
//							);
//							continue;
					}

					cdt_cnt +=1;
					cdt_sum[0] +=c_dt[0];
					cdt_sum[1] +=c_dt[1];
					cdt_sum[2] +=c_dt[2];
				}

			}

			if( cdt_cnt ) break;

		}
	//	if(iLsiDebugVariable == 2)printf("\n(1)cdt_cnt : %d\n",(int)cdt_cnt);
//		printf("%d : %f : %f : %f \n", cdt_cnt, cdt_sum[0], cdt_sum[1], cdt_sum[2]);
//		if(iLsiDebugVariable == 1)printf("\ncdt_cnt : %d\n",cdt_cnt);
		if( cdt_cnt != 0 ){
			int week;
			int tow=time2gpst(myObs[0].d[i].time,&week);
			double dTI;

	//		if(iLsiDebugVariable == 2)printf("\n(2)cdt_cnt : %d\n",(int)cdt_cnt);
			if(iLsiDebugVariable == 1){printf("^^^^^^^^^^^^^^^^^^^^^^Compare msg_04\n");}
			dTI = TI_Push(&ckTI,cdt_sum[2]/cdt_cnt);

//			dbgCV(129)printf("dTI:%.15lf,cdt_sum[2]:%.15lf,cdt_cnt:%.15lf\n",dTI,cdt_sum[2],cdt_cnt);

			v = ((cdt_sum[0]-cdt_sum[1])/(cdt_cnt*30))*1.e-7*1.e9;
			//v = ((cdt_sum[1]-cdt_sum[2])/(cdt_cnt*30))*1.e-7*1.e9;
                                 // slave cable delay  - master cable delay
			dbRlt[0] = v-dTI;//-(  myObs[1].InputCableDelay - myObs[0].InputCableDelay )-SYSOFFSET;
			//dbRlt[1] = push_v(&filter_M, dbRlt[0]);  // main data
			iCvValid = 10;

			dbRlt[1] = dbRlt[0];
			dbRlt[2] = v;
			dbRlt[3] = push_v(&filter_S, v);
			dbRlt[4] = dTI;
			dbRlt[5] = myObs[0].timeInterval[i];
			dbRlt[6] = myObs[1].timeInterval[j];

			if(fabs(dbRlt[1])>200){
				fp=fopen("/gpsclock/ORG/CV_Error","a+");
				if( fp == NULL ){
					printf( "Open Error... /gpsclock/ORG/CV_Error... for write+\n");
					//LogItString (LOG_CATA1_NORM, LOG_CATA2_FAIL, "File open for write");
					exit(-1);
				}
				printf("CV_Error\n");
				fprintf(fp,"%s",CV->Buf[CV->Tick].b);
			}

			do{
				dbgCV(3381)	printf("######################>>>>>>>>compare : %lf \n",dbRlt[1]);
				CV->CV_Value_Sum += dbRlt[1];
				CV->CV_Value_Sum_Count++;
			}while(0);


			if( VME.Data.TimeSync_Mode == VME_CV_MODE ){
				if(CONFIG.iCVType == 1){	//CV Slave
					VME.CV_Data.Compare_Error = 0;
				}
			}

/*
			for( ci = 0; ci< 7; ci++){
				printf("(dbRlt[%d]: %f)   ##", ci,dbRlt[ci] );
			}
			printf("\n");
*/
			if( x_value[0] == 0 ) 	t_time[0] = myObs[0].d[i].time;
			else      				t_time[1] = myObs[0].d[i].time;

			/////////
			/////////
			if( x_value[0] == 0 )
			{


//				WriteString("time,week,tow,match"
//				",*Time Diff,Diff.OfRef ,Diff.OfCounter,CounterMaster,CounterSlave\r\n", 0 );
//			s1 = AnsiString().sprintf(
//					"%s,%5d,%5d,(%d)"
//					",%.4f, %.4f, %.4f, %.4f, %.4f"
//			,time_str( myObs[0].d[i].time,3 ),week,tow ,(int)cdt_cnt
//			,dbRlt[1],dbRlt[3], dbRlt[4], dbRlt[5],dbRlt[6]);
//
//			s1 = AnsiString().sprintf(
//					"%s,%5d,%5d,(%d)"
//					",%.4f, %.4f, %.4f"
//			,time_str( myObs[0].d[i].time,3 ),week,tow ,(int)cdt_cnt
//			,cdt_sum[0]/cdt_cnt, cdt_sum[1]/cdt_cnt, dTI);
			}
			//
//			if( Form2->cbDetail->Checked ) WriteString(s1+s+"\r\n", 0 );
//			else WriteString(s1+"\r\n", 0 );
			/////////
			/////////

			x_value[0]++;

		}
		else{
			//if(iLsiDebugVariable == 2)printf("\n(3)cdt_cnt : %d\n",(int)cdt_cnt);
			//Printf1("tt");
		}
	}
//	Form2->pgr3rd->Visible = 0;
//	s1 = "number of ref. data: ";
//	s1 += myObs[0].iH;
//	s1 += ",  number of remote data: ";
//	s1 += myObs[1].iH;
//	s1 += "\r\nnumber of being caculated: ";
//	s1 += x_value[0];
//	s1 +="\r\n Start time:";
//	s1 += time_str( t_time[0],3 );
//	s1 +=" >> End time:";
//	s1 += time_str( t_time[1],3 );
//
//	Form2->mmResult->Text = s1;
}


